#include "Utilities/Utilities.h"
#include "rclcpp/rclcpp.hpp"
#include <time.h>
#include <sys/time.h> 

void Utility::hex_dump(const void *src, size_t length, size_t line_size, const char *prefix)
{
    int i = 0;
	const unsigned char *address = (unsigned char *)src;
	const unsigned char *line = address;
	unsigned char c;
	printf("%s | ", prefix);
	while (length-- > 0) {
		printf("%02X ", *address++);
		if (!(++i % line_size) || (length == 0 && i % line_size)) {
			if (length == 0) {
				while (i++ % line_size)
					printf("__ ");
			}
			printf(" | ");  /* right close */
			while (line < address) {
				c = *line++;
				printf("%c", (c < 33 || c == 255) ? 0x2E : c);
			}
			printf("\n");
			if (length > 0)
				printf("%s | ", prefix);
		}
	}
}

void Utility::hex_dump(const void *src, size_t length, const char *prefix)
{
    int i = 0;
	const unsigned char *address = (unsigned char *)src;
	// const unsigned char *line = address;
	// unsigned char c;
	printf("%s | ", prefix);
	while (length-- > 0) {
		printf("%02X ", *address++);
		if (!(++i % 16)) {
			printf("\n");
			if (length > 0)
				printf("%s | ", prefix);
		}
	}
}

long long int Utility::getTimestampUs() 
{
    struct timeval tv;
    if (gettimeofday(&tv, NULL) != -1) 
	{
        return ((long long)(tv.tv_sec)) * 1000000 + (long long)(tv.tv_usec);
    } 
	else 
	{
        perror("Failed to get timestamp");
        return -1;
    }
}

void Utility::print_time_consumption(const long long int& last_time_stamp)
{
	RCLCPP_INFO_STREAM(rclcpp::get_logger("Utility"), Utility::getTimestampUs() - last_time_stamp << " us" );
}
